/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.templates.HardwareMap;

/**
 *
 * @author jdmarler
 */
public class DriveTrain extends Subsystem {
    
    public static final double ENCODER_DISTANCE_PER_PULSE = 0.000623;
    
    RobotDrive Drive; 
    
    CANJaguar FrontLeft;
    CANJaguar RearLeft;
    CANJaguar FrontRight;
    CANJaguar RearRight;
    
    //Encoder LeftEncoder;
    //Encoder RightEncoder;
    
    public DriveTrain() {
       super("DriveTrain");
        
       try {
           FrontLeft = new CANJaguar(HardwareMap.FRONT_LEFT_JAGUAR);
           RearLeft = new CANJaguar(HardwareMap.REAR_LEFT_JAGUAR);
           FrontRight = new CANJaguar(HardwareMap.FRONT_RIGHT_JAGUAR);
           RearRight = new CANJaguar(HardwareMap.REAR_RIGHT_JAGUAR);  
        }
       catch (CANTimeoutException e){
           e.printStackTrace();
        }
            
        Drive = new RobotDrive(FrontLeft,RearLeft,FrontRight,RearRight);
                
        //LeftEncoder = new Encoder(HardwareMap.LEFT_DRIVE_ENCODER_CH_A,
          //  HardwareMap.LEFT_DRIVE_ENCODER_CH_B);
        
        //RightEncoder = new Encoder(HardwareMap.RIGHT_DRIVE_ENCODER_CH_A,
          //  HardwareMap.RIGHT_DRIVE_ENCODER_CH_B);
                
        //Sets distance in inches
        //This was obtained by finding the circumfrance of the wheels,
        //finding the number of encoder pulses per rotation,
        //and diving the circumfrance by that number.
        
        //LeftEncoder.setDistancePerPulse(ENCODER_DISTANCE_PER_PULSE);
        //RightEncoder.setDistancePerPulse(ENCODER_DISTANCE_PER_PULSE);
        
        //LeftEncoder.start();
        //RightEncoder.start();

   }
  
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public void TankDrive(double LeftValue,double RightValue){
        Drive.tankDrive(LeftValue, RightValue);
    }
    
                      
    public void ArcadeDrive(double MoveValue, double RotateValue){
        Drive.arcadeDrive(RotateValue, MoveValue);
    }
        
    //public double GetLeftEncoderValue(){
      //  return(LeftEncoder.getDistance());
    //}
    
    //public double GetRightEncoderValue(){
      //  return(RightEncoder.getDistance());
    //}
}
